#include <ros/ros.h>
#include "kinematic.h"
#include <ubt_core_msgs/JointCommand.h>
#include "webots_api/SceneSelection.h"
#include <opencv2/opencv.hpp>
#include "walker_srvs/leg_motion_MetaFuncCtrl.h"
#include <std_msgs/String.h>
#include <std_msgs/Int64.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

struct point3d_t
{
    float x;
    float y;
    float z;
};

struct point3d_t point3d;
ros::Subscriber image_sub_rgb;
ros::Subscriber image_sub_depth;

std::string leg_status = "standing";
void subWalkerStatus(const std_msgs::String &msgs) {
    leg_status = msgs.data;
}

geometry_msgs::Point center_data;
void subCenterData(const geometry_msgs::Point &msgs) {
    center_data = msgs;
}

int64_t step_num = 0;
void subWalkerStepNum(const std_msgs::Int64 &msgs) {
  step_num = msgs.data;
}

float cup_centre_x_rgb;
float cup_centre_y_rgb;
int point_received = 0;
float x_sum = 0;
float y_sum = 0;
float z_sum = 0;
bool point_already = false;

void head_imageCb_rgb(const sensor_msgs::Image msg)
{
    cv_bridge::CvImagePtr cv_ptr_rgb;
    cv::Mat img;
    cv_ptr_rgb = cv_bridge::toCvCopy(msg);
    img = cv_ptr_rgb->image;
    std::vector<cv::Mat> channels;
    cv::split(img, channels);
    cv::Mat threshold;
    cv::threshold(channels.at(2), threshold, 230, 255, CV_THRESH_BINARY);
    cv::Mat dstImage;
    cv::Mat normImage;//归一化后的图
    cv::cornerHarris(threshold, dstImage, 2, 3, 0.04);
    // 归一化与转换  
    cv::normalize(dstImage, normImage, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
    float final_x_L = 1000, final_y_L=1000, final_x_R=0, final_y_R=1000, y_min = 1000;
    std::vector<cv::Point> corners;

    // 进行绘制:将检测到的，且符合阈值条件的角点绘制出来  
    for (int j = 0; j < normImage.rows; j++)
    {
        for (int i = 0; i < normImage.cols; i++)
        {
            if ((int)normImage.at<float>(j, i) > 30 + 80)
            {
                if(j < y_min)
                    y_min = j;
                corners.push_back(cv::Point(i, j));
            }
        }
    }
    for(int i = 0; i < corners.size(); i++)
    {
        if(fabs(corners[i].y-y_min) < 10)
        {
            if(corners[i].x < final_x_L)
            {
                final_x_L = corners[i].x;
                final_y_L = corners[i].y;
            }
        }
        if(fabs(corners[i].y-y_min) < 10)
        {
            if(corners[i].x > final_x_R)
            {
                final_x_R = corners[i].x;
                final_y_R = corners[i].y;
            }
        }
    }
    cup_centre_x_rgb = (final_x_L+final_x_R)/2;
    cup_centre_y_rgb = (final_y_L+final_y_R)/2;
    // cv::circle(img, cv::Point((final_x_L+final_x_R)/2, (final_y_L+final_y_R)/2), 3, cv::Scalar(0, 255, 0), -1);
    // cv::imshow("RGB", img);
    // cv::waitKey(20);
}

void imageCb_depth(const sensor_msgs::Image msg)
{
    float f_x_rgb = 521.1711084;
    float f_y_rgb = 547.7089685;
    float c_x = 320.0;
    float c_y = 240;
    float f_x_depth = 572.5725821;
    float f_y_depth = 597.0187533;
    cv_bridge::CvImagePtr cv_ptr_rgb;
    cv::Mat depth_pic_depth;
    cv_ptr_rgb = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    depth_pic_depth = cv_ptr_rgb->image;
    //将rgb图的点（x，y）对齐到深度图中
    int cup_centre_x_depth = (int)(cup_centre_x_rgb - c_x) / f_x_rgb * f_x_depth + c_x;
    int cup_centre_y_depth = (int)(cup_centre_y_rgb - c_y) / f_y_rgb * f_y_depth + c_y;
    
    //输出测量点的深度
    float depth_point = depth_pic_depth.ptr<float>(cup_centre_y_depth)[cup_centre_x_depth];
    //将d转化为xyz
    float x = depth_point * sin((cup_centre_x_depth - 320)*58.4*M_PI/180/640);
    float y = depth_point * sin((240 - cup_centre_y_depth)*58.4*M_PI/180/480);
    float z = sqrt(depth_point * depth_point - x*x - y*y) * cos(20*M_PI/180);
    std::cout<<"x is:  "<< x <<"  || y is: "<< y <<"  || z is: "<< z <<std::endl; 
    if(point_received < 50)
    {
        x_sum = x_sum + x;
        y_sum = y_sum + y;
        z_sum = z_sum + z;
        point_received++;
    }
    else
    {
        point3d.x = x_sum /50;
        point3d.y = y_sum /50;
        point3d.z = z_sum /50;
        point_already = true;
        image_sub_depth.shutdown();
        image_sub_rgb.shutdown();
    }
    // cv::imshow("Depth", depth_pic_rgb);
    // cv::waitKey(20);
}

int main(int argc, char **argv) {
    Matrix_Init();
    ros::init(argc, argv, "PushCart_vision_node");
    ros::NodeHandle nh;

    ros::ServiceClient client = nh.serviceClient<webots_api::SceneSelection>("/walker/sence");
    ros::ServiceClient walk_client = nh.serviceClient<walker_srvs::leg_motion_MetaFuncCtrl>("/Leg/TaskScheduler");

    ros::Subscriber walker_status_sub = nh.subscribe("/Leg/leg_status", 10, &subWalkerStatus);
    ros::Subscriber walker_step_num_sub = nh.subscribe("/Leg/StepNum", 10, &subWalkerStepNum);
    ros::Subscriber center_data_sub = nh.subscribe("/Leg/dis_waist2ankle_left", 10, &subCenterData);
    image_sub_depth = nh.subscribe("/walker/camera/headDepth", 1,  &imageCb_depth);
    ros::Subscriber head_image_sub_depth = nh.subscribe("/walker/camera/headRGB", 1,  &head_imageCb_rgb);

    ros::Publisher walker_vel = nh.advertise<geometry_msgs::Twist>("/nav/cmd_vel_nav", 10);
    geometry_msgs::Twist vel_ctrl;

    walker_srvs::leg_motion_MetaFuncCtrl walker_walking;
    walker_walking.request.func_name = "dynamic";
    walker_walking.request.cmd = "start";
    
    webots_api::SceneSelection scene_selection;
    scene_selection.request.scene_name = "PushCart";
    scene_selection.request.nav = false;
    scene_selection.request.vision = true;
    
    std::cout << "Wait for SceneSelect Service" << std::endl;
    while(!client.call(scene_selection))
    {
        if(!ros::ok())
        {
            return 0;
        }
    }
    ROS_INFO("Scene select success!");

    ros::Publisher left_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftLimb/controller", 10);
    ubt_core_msgs::JointCommand left_arm_data;
    left_arm_data.command.resize(7);
    left_arm_data.mode = 5;

    ros::Publisher left_finger_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftHand/controller", 10);
    ubt_core_msgs::JointCommand left_finger_data;
    left_finger_data.command.resize(10);
    left_finger_data.mode = 5;

    ros::Publisher right_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/rightLimb/controller", 10);
    ubt_core_msgs::JointCommand right_arm_data;
    right_arm_data.command.resize(7);
    right_arm_data.mode = 5;

    ros::Publisher right_finger_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/rightHand/controller", 10);
    ubt_core_msgs::JointCommand right_finger_data;
    right_finger_data.command.resize(10);
    right_finger_data.mode = 5;

    Eigen::VectorXd left_joints(7);
    left_joints << 0,0,0,0,0,0,0;
    Eigen::VectorXd right_joints(7);
    right_joints << 0,0,0,0,0,0,0;

    Eigen::Vector3d position, orientation;

    ros::Rate loop_rate(1000);
    double time_count = 0.0;
    double tim_cnt = 0;
    float x_bias;
    int step_num_to_go = 0;
    float step_len = 0;
    float control_distance = 2.31;   // 控制模式下目标的距离
    bool second_go = false;
    bool first_go = true;
    while(ros::ok())
    {
        time_count += 0.001;
        if(time_count < 1)
        {
            position << 400-100*(time_count)+x_bias,275*(time_count),400-350*(time_count);
            if(leftArmInverse(position, orientation, left_joints, false) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -400+100*(time_count)+x_bias,275*(time_count),400-350*(time_count);
            if(rightArmInverse(position, orientation, right_joints, false) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }
        }
        else if(time_count < 1.5)
        {
            position << 300-360*(time_count-1)+x_bias, 275+100*(time_count-1), 50+200*(time_count-1);
            orientation << 0,M_PI/2,-M_PI/2;
            if(leftArmInverse(position, orientation, left_joints, true) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -300+360*(time_count-1)+x_bias, 275+100*(time_count-1), 50+200*(time_count-1);
            orientation << -0,M_PI/2,M_PI/2;
            if(rightArmInverse(position, orientation, right_joints, true) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }
        }
        else if(second_go == false)
        {
            position << 120+x_bias, 325, 150;
            orientation << 0,M_PI/2,-M_PI/2;
            if(leftArmInverse(position, orientation, left_joints, true) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -120+x_bias, 325, 150;
            orientation << -0,M_PI/2,M_PI/2;
            if(rightArmInverse(position, orientation, right_joints, true) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }
        }
        if(first_go == true && (leg_status == "standing" || leg_status == "standInit"))
        {
            walk_client.call(walker_walking);
            ros::spinOnce();
            loop_rate.sleep();
            continue;
        }
        first_go = false;
        if(leg_status == "dynamic" || leg_status == "standing")
        {
            x_bias = (center_data.y - 0.0798)*1000;
        }
        else
        {
            x_bias = 0;
        }
        if(!point_already){
            ros::spinOnce();
            loop_rate.sleep();
            continue;
        }
        // 此处根据目标的距离计算要走几步，以及要走的步数
        if(point3d.z <= control_distance + 0.32)
        {
            step_num_to_go = 1;
            step_len = point3d.z - control_distance;
        }
        else if(point3d.z <= control_distance + 0.64)
        {
            step_num_to_go = 2;
            step_len = (point3d.z - control_distance)/2;
        }
        else if(point3d.z <= control_distance + 0.96)
        {
            step_num_to_go = 3;
            step_len = (point3d.z - control_distance)/3;
        }
        else if(point3d.z <= control_distance + 1.28)
        {
            step_num_to_go = 4;
            step_len = (point3d.z - control_distance)/4;
        }
        if(second_go == false)
        {
            if(step_num <= step_num_to_go)
            {
                if(leg_status == "dynamic")
                {
                    vel_ctrl.linear.x = step_len;
                    walker_vel.publish(vel_ctrl);
                }
            }
            else
            {
                walker_walking.request.func_name = "dynamic";
                walker_walking.request.cmd = "stop";
                walk_client.call(walker_walking);
                if(fabs(center_data.x) > 0.01)
                {
                    ros::spinOnce();
                    loop_rate.sleep();
                    continue;
                }
                else
                {
                    second_go = true;
                }
            }
            ros::spinOnce();
            loop_rate.sleep();
            continue;
        }

        if(leg_status == "standing" || leg_status == "stopping")
        {
            walker_walking.request.func_name = "dynamic";
            walker_walking.request.cmd = "start";
            walk_client.call(walker_walking);
        }

        tim_cnt += 0.001;
        if(tim_cnt < 0.2)
        {
            position << 120+x_bias, 325, 150+40*tim_cnt/0.2;
            orientation << 0,M_PI/2,-M_PI/2;
            if(leftArmInverse(position, orientation, left_joints, true) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -120+x_bias, 325, 150+40*tim_cnt/0.2;
            orientation << -0,M_PI/2,M_PI/2;
            if(rightArmInverse(position, orientation, right_joints, true) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }
        }
        else
        {
            position << 120+x_bias, 325, 190;
            orientation << 0,M_PI/2,-M_PI/2;
            if(leftArmInverse(position, orientation, left_joints, true) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -120+x_bias, 325, 190;
            orientation << -0,M_PI/2,M_PI/2;
            if(rightArmInverse(position, orientation, right_joints, true) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }

            float angle = 1.5;
            left_finger_data.command[0] = angle;
            left_finger_data.command[1] = angle;
            left_finger_data.command[2] = angle;
            left_finger_data.command[3] = angle;
            left_finger_data.command[4] = angle;
            left_finger_data.command[5] = angle;
            left_finger_data.command[6] = angle;
            left_finger_data.command[7] = angle;
            left_finger_data.command[8] = angle;
            left_finger_data.command[9] = angle;
            left_finger_pub.publish(left_finger_data);

            right_finger_data.command[0] = angle;
            right_finger_data.command[1] = angle;
            right_finger_data.command[2] = angle;
            right_finger_data.command[3] = angle;
            right_finger_data.command[4] = angle;
            right_finger_data.command[5] = angle;
            right_finger_data.command[6] = angle;
            right_finger_data.command[7] = angle;
            right_finger_data.command[8] = angle;
            right_finger_data.command[9] = angle;
            right_finger_pub.publish(right_finger_data);

            if(leg_status == "dynamic")
            {
                vel_ctrl.linear.x = 0.2;
                walker_vel.publish(vel_ctrl);
            }
        }
        
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
